#include <Eigen/Eigen>
#include <bits/stdc++.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <geometry_msgs/TwistStamped.h>

ros::Publisher pose_pub,vel_pub;
geometry_msgs::PoseStamped current_p;
geometry_msgs::TwistStamped current_v;

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
  current_p=*msg;
  current_p.header.stamp=ros::Time::now();
}

void velCallback(const geometry_msgs::TwistStamped::ConstPtr &msg) {
  current_v=*msg;
  current_v.header.stamp=ros::Time::now();
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "send_vicon");
  ros::NodeHandle nh("~");
  ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(
      "/mocap/pose", 1, positionCallback);
  ros::Subscriber vel = nh.subscribe<geometry_msgs::TwistStamped>(
      "/mocap/vel", 1, velCallback);
  pose_pub =
      nh.advertise<geometry_msgs::PoseStamped>("/global_position", 1);
  vel_pub =
      nh.advertise<geometry_msgs::TwistStamped>("/global_vel", 1);
  const int LOOPRATE = 30;
  ros::Rate loop_rate(LOOPRATE);
  while(true){
    pose_pub.publish(current_p);
    vel_pub.publish(current_v);
    loop_rate.sleep();
    ros::spinOnce();
  }
}